#
# Copyright (c) 2012 The Chromium OS Authors. All rights reserved.
# Use of this source code is governed by a BSD-style license that can be
# found in the LICENSE file.
#
#
# Toshiba Machine Co, LTD.
# ROIBOT
# BA Series
# CA10-M00
#
# Industrial robot interface mode control module
#

import select
import serial
import time

#
# Exceptions
#
class ROIbotException(Exception):
    """Base class for robot related exception."""

class ROIbotParameterException(ROIbotException):
    """Parameter exception."""

class ROIbotTimeoutException(ROIbotException):
    """Timeout exception."""

class ROIbotModeException(ROIbotException):
    """"Operation not permitted in this mode."""

class ROIbotUnrecoverableErrorException(ROIbotException):
    """"Software could not clear an error."""

readTimeoutError = ROIbotTimeoutException("Read timeout")
hostModeError = ROIbotModeException("Command invalid outside host mode")
writeProtectError = ROIbotModeException("EEPROM is write protected")
unrecoverableError = ROIbotUnrecoverableErrorException("Unrecoverable")


#
# Implementation class
#
# Exceptions:   ROIbotModeException     Thrown when a requested operation is
#                                       not permitted in the current mode.
#
#               ROIbotTimeoutException  A read attempt had timed out; if this
#                                       was a long duration operation,
#                                       consider subclassing and adding a
#                                       utility function to set a higher
#                                       timeout for the operation.
#
#               ROIbotReadOnlyException We maintain a synthetic EEPROM protect
#                                       flag to prevent unintentional calls to
#                                       modify the EEPROM content.
#
class ROIbot(serial.Serial):

    _AdhocTestON = True
    _writeEEPROM = False
    _hostModeON = False
    _line_number = 1
    _point_number = 1
    _speed_number = 1
    _points = {}
    _speeds = {}
    _current_speed = -1
    _MAX_SPEED_NUMBER = 10
    _RESPONSE_TIMEOUT = 2   # In seconds

    #
    # Utility functions
    #
    def enableWriteEEPROM(self):
        """Enable writing the EEPROM by setting the soft write protect to
           True.  The default is that the soft write protect is enabled,
           and commands which would result in the EEPROM being written will
           throw the exception ROIbotReadOnlyException."""
        self._writeEEPROM = True

    def disableWriteEEPROM(self):
        """Disable writing the EEPROM by setting the soft write protect to
           False.
        """
        self._writeEEPROM = False

    def assertCancelError(self):
        """Attempts to clear the error status, if any, twice.  If an error
           exists after that, it raises an unrecoverableError exception.
        """
        if not self.execCancelError():
            if not self.execCancelError():
                raise unrecoverableError

    def assertReset(self):
        """Resets the robot.  On failure raises an unrecoverableError
           exception.
        """
        if not self.execReset():
            if not self.execReset():
                raise unrecoverableError

    def sendCommand(self, *args):
        """Send a command to the robot and wait for a response; a command
           is of the form:

                @<four character command>[<space><data>...]<cr><lf>

           All commands Receive responses in the form of a command, with the
           canonical response being a success or failure indicator:

               @OK<cr><lf>
               @NG<cr><lf>

           The function returns a list consisting of a result string followed
           by a (potentially empty) list of additional data items.
        """
        cmdlist = [str(arg) for arg in args]
        cmd = ' '.join(cmdlist)
        self.write("@" + cmd + "\r\n")

        # Wait for a few seconds to see if there is a respose.  If not, fail
        rfds, wfds, efds = select.select([self], [], [], self._RESPONSE_TIMEOUT)
        if not self in rfds:
            raise readTimeoutError

        # Get whatever data the robot sent back
        response = self.readline().lstrip("@").rstrip("\r\n").split(" ")
        code = response.pop(0)
        if (code == ''):
            raise readTimeoutError
        result = [code, response]
        return result

    # Returns True on success; test for failure with "if not <command>:"
    def TFResult(self, tuple):
        if (tuple[0] == 'OK'):
            return True
        return False

    #
    # Assertion convenience functions for software enforced state flags
    #
    def assertHostModeON(self):
        if (self._hostModeON == False and self._AdhocTestON == False):
            raise hostModeError

    def assertWritableEEPROM(self):
        if (self._writeEEPROM == False and self._AdhocTestON == False):
            raise writeProtectError

    #
    # Mode commands
    #
    # Note: These include status requests, most likely because they had no
    # other reasonable place to group them.
    #
    # As a general rule, all mode commands other than host mode control or
    # status requests require the robot to be in host mode.
    #
    def modeRequestSystemParameter(self):
        """Request system parameter.  This returns the robot type information,
           the controller version, the language used by the teaching pad
           (generally irrelevant), and information about up to 4 Axis of
           motion.
        """
        return self.sendCommand("SYSP")

    def modeRequestControllerVersion(self, station=0):
        """Request controller version information from up to five stations
           in the range 0..4; if no station number is specified, the
           version information for station 0 will be reported by default.

           The result is a tuple of the form:

               ["NO=<station number>", "VER=<2 digits>.<2 digits>"]

           returned.
        """
        return self.sendCommand("CVER", "NO=" + str(station))

    def modeRequestRobotType(self, axis=1, robotType=0):
        """Request robot type information from up to four axis in the
           range 1..4, and a asserted robotType in the range of 0-99999.
           If no axis is specified, the type information for axis 1 will be
           reported by default.  If no robotType is specifidm the type will
           be tested vs. a default type of 0.

           The result is a tuple of the form:

            ["AX<axis>", "RTYP=<6 digits>"]

           returned.
        """
        return self.sendCommand("TYPE", "AX" + str(axis),
                                "RTYP=%06d" % robotType)

    def modeRequestStatus(self, statusNumber=0):
        """Request controller status information one of five status registers
           in the range 0..4; if no statusNumber is specified, the default
           is to return stutus "0".

           The result is a tuple of the form:

                ["SNO=<statusNumber>", "ST1=<%02x>", "ST2=<%02x>"]

           returned.
        """
        return self.sendCommand("STAS", "SNO=" + str(statusNumber))

    def parseStatusCode(self, response):
        """ Parse out the status code from a raw response.  Takes a string of
            the form 'STR1=13' and returns the status code
        """
        return int(str(response).split("=")[1], 16)

    def modeHostON(self):
        """Enter host mode.  This mode is required to use most commands."""
        result = self.TFResult(self.sendCommand("HSON"))
        if (result):
            self._hostModeON = True
        return result

    def modeHostOFF(self):
        """Exit host mode.  Most commands will be unavailable when not in
           host mode.
        """
        result = self.TFResult(self.sendCommand("HSOF"))
        if (result):
            self._hostModeON = False
        return result

    def modeProgram(self):
        """Enter program mode.  Program mode may be cancelled via modeHostON.

           You must be in in Host mode to enter program mode.
        """
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("MPRO"))

    def modeStep(self):
        """Enter step mode."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("MSTP"))

    def modeAutomatic(self):
        """Enter Automatic mode."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("MAUT"))

    def modeSequential(self):
        """Enter sequential mode.  Sequential mode may be cancelled via
           modeHostON.

           You must be in in Host mode to enter sequential mode.
        """
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("MSEQ"))

    def modePalletizing(self):
        """Enter palletizing mode.  Palletizing mode may be cancelled via
           modeHostON.

           You must be in in Host mode to enter palletizing mode.
        """
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("MPLE"))

    #
    # Parameter commands
    #
    # Read and write parameters.
    #
    # Writing is restricted to writeEEPROM mode being enabled.
    #
    def parameterRead(self, parameter):
        """Read parameters.  Parameters are described in section 2-4 of the
           COMPO ARM RS232C Communications Specifications manual.  There are
           a significant number of them, and they may only be changed in
           EEPROM write enabled mode, so they are not specifically documented
           here at this time.  It's possible that at some future point that
           parameter decoding functions will be made available using an
           auxillary class.
        """
        return self.sendCommand("RPAR " + str(parameter))

    def parameterWrite(self, parameter, *args):
        """Write parameters.  Parameters are described in section 2-4 of the
           COMPO ARM RS232C Communications Specifications manual.  There are
           a significant number of them, and they may only be changed in
           EEPROM write enabled mode, so they are not specifically documented
           here at this time.  It's possible that at some future point that
           parameter encoding functions will be made available using an
           auxillary class.
        """
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("WPAR", str(parameter), *args))


    #
    # Text editing commands
    #
    # These commands deal predominantly with reading and writing of the
    # EEPROM and memory card contents.  Use of memory card commands
    # requires installation of the optional memory card module.
    #
    # EEPROM write operations are restricted to writeEEPROM mode being
    # enabled.
    #
    def textInitializeMemory(self, regionCode):
        """Initialize memory.  The parameter is a region code in the range
           0..6, with the values having the following meanings:

               0    Memory clear
               1    Sequential clear
               2    Palletizing clear
               3    Sequential/Palletizing clear
               4    Easy/Point table clear
               5    Reserved (do not send this to avoid future issues)
               6    Point table clear

           Note that Sequential and Point table only apply to the currently
           set tasks, rather than all tasks.
        """
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("MINT", "PR=" + str(regionCode)))

    def textCopySequentialStep(self, sourceTask, startStep, endStep,
                       targetTask, targetStep):
        """Copy a set of steps from one task to another."""
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("CPSS",
                                              "TASK=%02d" % sourceTask,
                                              "STA=%04d" % startStep,
                                              "END=%04d" % endStep,
                                              "TEASK=%02d" % targetTask,
                                              "TOP=%04d" % targetStep))

    def textInsertSequentialLine(self, insertionStep):
        """Insert a step at the designated step."""
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("ISTX", "%04d" % insertionStep))

    def textDeleteSequentialLine(self,deletionStep):
        """Delete the designated step"""
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("DSTX", "%04d" % deletionStep))

    def textSearchSequentialTag(self, searchTag):
        """Search for the specified text tag; tags are "goto" labels."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("SSTG", "TAG=%03d" % searchTag))

    def textDeleteSequentialBlock(self, startStep, endStep):
        """Delete a sequential block of steps."""
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("DSTL",
                                              "STA=%04d" % startStep,
                                              "END=%04d" % endStep))

    def textCopyPalletizingProgram(self, sourceProgram, destinationProgram):
        """Copy one palletizing program to another."""
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("CPPL",
                                              "STA=%02d" % sourceProgram,
                                              "TOP=%02d" % destinationProgram))

    def textCopyEasyProgram(self, sourceProgram, destinationProgram):
        """Copy one easy program to another."""
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("CESY",
                                              "STA=%02d" % sourceProgram,
                                              "TOP=%02d" % destinationProgram))

    def textWriteEasy(self, program, startTag, loopCount, endTag):
        """Write easy text."""
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("WESY",
                                              "PNO=%02d" % program,
                                              "ST=000",
                                              "START=%03d" % startTag,
                                              "LOOP=%04d" % loopCount,
                                              "END=%03d" % endTag))

    def textReadEasy(self):
        """Read easy text."""
        return self.sendCommand("RESY", "PNO=%02d" % program, "ST=000")

    def textWriteSequential(self, *args):
        """Enter sequential text entry mode.  It's strongly suggested in
           the documentation that this mode completes when automatic mode
           is entered.  While in this mode, additional sequential writes
           result in program storage of the arguments.

           TODO(tlambert): textWriteSequentialStep()
                           textWriteSequentialEnd()
           """
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("WSTX", *args))

    def textReadSequential(self):
        """Read sequential text.  Start reading back sequential program steps.

           TODO(tlambert): textReadSequentialStep()
           """
        return self.sendCommand("RSTX", "0001")

    def textWritePalletizing(self):
        """Enter palletizating text entry mode.

           TODO(tlambert): textWritePalletizingStep()_
                           textWritePalletizingEnd()
           """
        self.assertHostModeON()
        self.assertWritableEEPROM()
        return self.TFResult(self.sendCommand("WPLT",
                                              "99",
                                              "SNO=01",
                                              "TAG=001",
                                              "MOD=M-M"))


    def textReadPalletizing(self):
        """Read [alletizing text.  Start reading program steps.

           TODO: textReadPalletizingStep()
           """
        return self.sendCommand("RPLT", "99", "SNO=01", "TAG=001", "MOD=M-M")

    #
    # Memory card specific commands
    #
    # This sections is offset separately due to the optional nature of
    # the memory card module.
    #
    def textWriteMemoryCard(self):
        """Write to memory card."""
        return self.TFResult(self.sendCommand("WMCA"))

    def textReadMemoryCard(self):
        """Read from memory card."""
        return self.TFResult(self.sendCommand("RMCA"))


    #
    # Execution control
    #
    # These operations are immediately carried out by the robot.  Not all
    # of them require host mode.  Of all of them, execWriteOverride()
    # also requires write enabling the EEPROM.
    #
    # In general, it's possible to call into other modes and perform port
    # output, or potentially even damage the robot using these commands,
    # so caution should be exercised in their use.
    #
    def execReturnToOrigin(self):
        """Return all positioning servos to their origin (power on) state."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("HOME"))

    def execStartSequential(self, step):
        """Start sequential beginning at the specified step."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("SPST", "%04d" % step))

    def execSetPalletizingProgram(self, progran):
        """Set the current palletizing program.  Palletizing programs
           are in the range 01..16 and must be pre-programmed into the
           EEPROM.
           """
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("SPLN", "PR=%02d" % progran))

    def execStartPalletizing(self):
        """Start the currently selected palletizing program."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("PPST"))

    def execSetEasyProgram(self, program):
        """Set the current easy program.  Easy programs are in the range
           01..08 and must be pre-programmed into the EEPROM.
           """
        return self.TFResult(self.sendCommand("EXTP", "PT=%03d" % point))

    def execStartEasy(self):
        """Start the currently selected easy program."""
        return self.TFResult(self.sendCommand("SESY"))

    def execRequestPalletizingData(self):
        """Read out the palletising data; this data is set using text
           commands.
           """
        return self.TFResult(self.sendCommand("RPLD"))

    def execStop(self):
        """Stop sequential, palletizing, easy, or external point designation
           operations in progress.
           """
        return self.TFResult(self.sendCommand("STOP"))

    def execJog(self, X="SP", Y="SP", Z="SP", R="SP"):
        """Move the robot in up to four axes. Motion will occur one axis
           at a time.  Multiple axes can not be moved at the same time.
           """
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("JGST",
                                              "X=" + str(X),
                                              "Y=" + str(Y),
                                              "Z=" + str(Z),
                                              "R=" + str(R)))

    def execDirectPortOutput(self, station, portNumber, outputDesignation):
        """Direct port output."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("DOUT",
                                              "STN=" + str(station),
                                              "PN%02d=" % portNumber,
                                              "STR=" + str(outputDesignation)))

    def execWriteOverride(self, override):
        """Write override.  Only valid when the program is stopped."""
        self.assertWritableEEPROM()
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("WOVR", "OV=%03d" % override))

    def execReadOverride(self):
        """Read override."""
        return self.sendCommand("ROVR")

    def execReset(self):
        """Reset."""
        return self.TFResult(self.sendCommand("REST"))

    def execCancelError(self):
        """Cancel an outstanding error condition."""
        return self.TFResult(self.sendCommand("CERR"))

    def execStartExternalPointDesignation(self, point):
        """Start external point designation given a coordinate table entry
           number in the range 001..999.
           """
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("EXTP", "PT=%03d" % point))

    def execServoON(self):
        """Servo on."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("SVON"))

    def execServoOFF(self):
        """Servo off."""
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("SVOF"))

    def execRequestEasyExecutionData(self):
        """Read out easy execution status."""
        return self.sendCommand("RESD")

    def execChangeTaskNumber(self, task):
        """Change the task number to the specified value.  Task values are
           in the range 01..04.
           """
        return self.TFResult(self.sendCommand("CTSK", "PR=%02d" % task))

    def execSynchronizedOriginSearch(self, axis):
        """Synchronized origin search.  This command is only valid when
           communicating with a CA20-M00/M01 robot.
           """
        self.assertHostModeON()
        return self.TFResult(self.sendCommand("SORG", str(axis)))


    #
    # Monitor commands
    #
    def monRequestInputData(self, station, screen):
        """Request input data.  Reads input from a station in the range 0..4
           from a screen in the range 1..7 (stations in the range 1..4 are
           fixed at screen 1).  Port data is returned in the response.
           """
        return self.sendCommand("MINP",
                                "SNO=" + str(station),
                                "GN=" + str(screen))

    def monRequestOutputData(self, station, screen):
        """Request ouput data from a station in the range 0..4 from a screen
           in the range 1..5 (stations in the range 1..4 are fixed at screen
           1).  Port data is returned in the response.
           """
        return self.sendCommand("MOUT",
                                "SNO=" + str(station),
                                "GN=" + str(screen))

    def monInternalPortData(self, station, screen):
        """Internal port data request from a station (must be 0) screen
           (must be 1).  Port information for up to 4 ports is returned.
           """
        # TODO(tlambert): assert the requirements.
        return self.sendCommand("MNIN",
                                "SNO=" + str(station),
                                "GN=" + str(screen))

    def monRequestPresentPosition(self):
        """Request present position.  Returns the current X, Y, Z, and
           rotational coordinates.  Not all robots support all coordinates.
           """
        return self.sendCommand("MPST")

    def getCurrentPosition(self):
        """Requests the position and returns it as an (x, y, z) tuple of
           floats.  This eliminates the parsing issues and the unused r
           dimension since we don't have that on our robot anyway.
           """
        response_type, response_data = self.monRequestPresentPosition()
        x = float(response_data[0].split("=")[1])
        y = float(response_data[1].split("=")[1])
        z = float(response_data[2].split("=")[1])
        return (x, y, z)

    def monRequestPresentOffsetValue(self):
        """Request present offset value.  Returns the current X, Y, Zm and
           rotational offsets.  Not all robots support all coordinates.
           """
        return self.sendCommand("MOFF")

    def monRS232CCoordinateDataSettings(self, task, x, y, z, rotation,
                                        velocity):
        """Set the current X, Y, Z, and rotation at the velocity specified
           in the speed table entry requested.  Coordinates are in the range
           -8000.00..+8000.00; the speed table entry is in the range 00..10.
           """
        return self.TFResult(self.sendCommand("MRSS",
                                              "TASK=%02d" % task,
                                              "X=%07.02f" % x,
                                              "X=%07.02f" % y,
                                              "X=%07.02f" % z,
                                              "X=%07.02f" % rotation,
                                              "X=%02" % velocity))

    def monRequestCounterValue(self, counter):
        """Request the current value of the specified counter in the range
           01..99.
           """
        return self.sendCommand("MCNT", "CN=%02d" % counter)

    def monRequestTimerValue(self, timer):
        """Request the current value of the specified timer in the range
           1..9.
           """
        return self.sendCommand("MTMR", "TN=" + str(timer))

    def monCounterDirectSet(self, counter, value):
        """Set the specified counter in the range 01..99 to the specified
           value in the range 0000..9999.
           """
        return self.TFResult(self.sendCommand("MCST",
                                              "CN%02d=%03d" % (counter, value)))

    def monRequestErrorPointNumber(self):
        """Request the error point number."""
        return self.sendCommand("MERP")

    def monReadTaskNumberReadCurrentStep(self):
        """Read the current task number; results are in the range 01..04."""
        return self.sendCommand("MTSK")

    def monReadCurrentStep(self):
        """Read the current step number; results are in the range
           0001..2500.
           """
        return self.sendCommand("RSTX", "0000")

    def monReadCCLinkStatus(self, index):
        """Read CC-Link status.  This command is only valid when
           communicating with a CA20-M00/M01 robot.
           """
        return self.sendCommand("MBUS", "C" + str(step))

    def monErrorHistoryRequest(self):
        """Request error history."""
        return self.sendCommand("EHTR", "NO=01")

    # High level EEPROM programming instructions
    #
    # The follow functions wrap the commands for writing a program to the
    # EEPROM of the robot.  They all will make multiple attempts to issue
    # their command, and raise an exception if they are unsuccessful.
    def addCmd(self, cmd):
        """Upload a new command into the next spot in the eeprom.  This adds a
        new instruction to the program in the robots memory.
        """
        if type(cmd) is tuple:
            cmd = " ".join(cmd)

        print "ADDING:  %04d %s" % (self._line_number, cmd)
        self.tryOrFail(self.textWriteSequential,
                       "%04d" % self._line_number, cmd)
        self._line_number += 1

    def setParam(self, cmd):
        """Set a parameter's value.  This can be any "parameter" in the robot,
        such as altering speed profiles or point tables, etc.
        """
        print "SETTING: %s" % cmd
        self.tryOrFail(self.parameterWrite, cmd)

    def setSpeed(self, speed):
        """Change the robot's operating speed.  This will add new entries to the
        speed table and new program instructions only if it needs to.
        """
        if speed <= 0 or speed > 999:
            return

        if self._current_speed == speed:
            return

        if not speed in self._speeds:
            if self._speed_number > self._MAX_SPEED_NUMBER:
                print "WARNING: Unable to add speed %d to the speed table!"
                return
            self._speeds[speed] = self._speed_number
            self.setParam("T2 V%02d=%06.01f" % (self._speed_number, speed))
            self._speed_number += 1

        self.addCmd("SPD V=%02d" % self._speeds[speed])
        self._current_speed = speed


    def addPoint(self, x, y, z):
        """Add a new point to the point table (returns the point number)."""
        if not (x, y, z) in self._points:
            self.setParam("T1 PT=%03d " % self._point_number +
                          "X=+%07.02f " % x +
                          "Y=+%07.02f " % y +
                          "Z=+%07.02f " % z +
                          "R=+0000.00")
            self._points[(x, y, z)] = self._point_number
            self._point_number += 1
        return self._points[(x, y, z)]

    def tryOrFail(self, fn, *args, **kwargs):
        """Make multiple attempts to call a function fn().  The function should
        return True or False.  If after NUM_RETRIES attempts, there still
        hasn't been a success, raise an exception.
        """
        NUM_RETRIES = 2
        for i in range(NUM_RETRIES):
            result = fn(*args, **kwargs)
            if result:
                return result
            time.sleep(1)
        raise unrecoverableError

    def addMoveCommand(self, x, y, z, speed, accuracy="POST"):
        """Add the command necessary to tell the robot to move to the point
        (x, y, z) at the specified speed.  This conserves eeprom space and
        will not add unnecessary instructions.
        """
        self.setSpeed(speed)
        point_index = self.addPoint(x, y, z)
        self.addCmd("MOVP a PT=%03d CN=00 S V=00 %s" % (point_index, accuracy))
# End
